﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Text;

namespace LasRendering
{
    public class PriorityQueue<P, T> : IEnumerable<T>
    {
        private int size;
        private P[] priorities;
        private T[] values;
        private int version;

        public PriorityQueue()
        {
            this.size = 0;
            this.priorities = new P[0];
            this.values = new T[0];
        }

        public PriorityQueue(int capacity)
        {
            this.size = 0;
            this.priorities = new P[capacity];
            this.values = new T[capacity];
        }

        public void Push(P priority, T item)
        {
            int num = Array.BinarySearch<P>(this.priorities, 0, this.size, priority);
            int index = num < 0 ? ~num : num + 1;

            if (this.size == this.values.Length)
            {
                this.EnsureCapacity(this.size + 1);
            }
            if (index < this.size)
            {
                Array.Copy(this.priorities, index, this.priorities, index + 1, this.size - index);
                Array.Copy(this.values, index, this.values, index + 1, this.size - index);
            }
            this.priorities[index] = priority;
            this.values[index] = item;
            this.size++;
            this.version++;
        }

        public T Pop()
        {
            this.size--;
            T item = this.values[this.size];
            this.priorities[this.size] = default(P);
            this.values[this.size] = default(T);
            this.version++;
            return item;
        }

        public T Peek()
        {
            return this.values[this.size - 1];
        }

        public int Count
        {
            get
            {
                return this.size;
            }
        }

        public P MinPriority
        {
            get
            {
                return this.priorities[0];
            }
        }

        public P MaxPriority
        {
            get
            {
                return this.priorities[this.size - 1];
            }
        }

        private void EnsureCapacity(int min)
        {
            int capacity = (this.priorities.Length == 0) ? 4 : (this.priorities.Length * 2);
            if (capacity < min)
            {
                capacity = min;
            }

            if (capacity != this.values.Length)
            {
                if (capacity > 0)
                {
                    P[] newPriorities = new P[capacity];
                    T[] newValues = new T[capacity];
                    if (this.size > 0)
                    {
                        Array.Copy(this.priorities, 0, newPriorities, 0, this.size);
                        Array.Copy(this.values, 0, newValues, 0, this.size);
                    }
                    this.priorities = newPriorities;
                    this.values = newValues;
                }
                else
                {
                    this.priorities = new P[0];
                    this.values = new T[0];
                }
            }
        }

        private class Enumerator : IEnumerator<T>
        {
            private PriorityQueue<P, T> queue;
            private int index = 0;
            private T value;
            private int version;

            internal Enumerator(PriorityQueue<P, T> q)
            {
                this.queue = q;
                this.index = 0;
                this.value = default(T);
                this.version = q.version;
            }

            public T Current
            {
                get
                {
                    return this.value;
                }
            }

            public void Dispose()
            {
                this.index = 0;
                this.value = default(T);
            }

            object IEnumerator.Current
            {
                get
                {
                    return this.value;
                }
            }

            public bool MoveNext()
            {
                if (this.version != this.queue.version)
                {
                    throw new InvalidOperationException("InvalidOperation_EnumFailedVersion");
                }
                if (this.index < this.queue.Count)
                {
                    this.value = this.queue.values[this.index];
                    this.index++;
                    return true;
                }
                this.index = this.queue.Count + 1;
                this.value = default(T);
                return false;
            }

            public void Reset()
            {
                if (this.version != this.queue.version)
                {
                    throw new InvalidOperationException("InvalidOperation_EnumFailedVersion");
                }
                this.index = 0;
                this.value = default(T);
            }
        }

        public IEnumerator<T> GetEnumerator()
        {
            return new Enumerator(this);
        }

        IEnumerator IEnumerable.GetEnumerator()
        {
            return this.GetEnumerator();
        }
    }
}
